//#include "I2Cdev.h"
//#include "MPU6050_6Axis_MotionApps20.h"
#include "Wire.h"
#include "sensor.h"

//#if I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE
//#include "Wire.h"
//#endif

//MPU6050 mpu;

//#define OUTPUT_READABLE_YAWPITCHROLL

#define LED_PIN 13 // (Arduino is 13, Teensy is 11, Teensy++ is 6)
bool blinkState = false;

#include <Servo.h> 
Servo myservo;
int val;

unsigned long timer;
unsigned long lastBlink, lastReadPot;
int intervall = 500;


// ================================================================
// ===                      INITIAL SETUP                       ===
// ================================================================

void setup() {
  Wire.begin();
  TWBR = 24; // 400kHz I2C clock (200kHz if CPU is 8MHz)

  Serial.begin(115200);
  while (!Serial); // wait for Leonardo enumeration, others continue immediately

  /*myservo.attach(9);
   myservo.write(90);
   delay(1000);*/


  sensorInit();

  // configure LED for output
  pinMode(LED_PIN, OUTPUT);
}



// ================================================================
// ===                    MAIN PROGRAM LOOP                     ===
// ================================================================

void loop() {
  // if programming failed, don't try to do anything
  if (!dmpReady) {
     Serial.println("morto"); 
    return;
  }



  sensorRead();


  if(millis() - lastBlink > intervall) {
    lastBlink = millis();
    // blink LED to indicate activity
    blinkState = !blinkState;
    digitalWrite(LED_PIN, blinkState);
  }
#ifdef POT
  if(millis() - lastReadPot > 15) {
    lastReadPot = millis();
    val = analogRead(0);
    val = map(val, 0, 1023, 0, 180);
    myservo.write(val);
  }
#endif

  timer = millis();
}

